#include "bezier.h"
#include "math.h"
#include <math.h>
#include <winerror.h>

void Bezier::Clear()
{
	m_Points.Empty(); 
	m_Direction.Empty(); 
}

void Bezier::AddPoint(const Vector3f& point, const Vector3f& dir)
{
	m_Points.Add(point); 
	m_Direction.Add(dir); 
}

unsigned int Bezier::GetPointCount()
{
	return m_Points.Count(); 
}

HRESULT Bezier::Evaluate(Vector3f* pPosition, const float fFrac)
{
	const unsigned int uiPointCount = m_Points.Count(); 
	if(uiPointCount > 1 && pPosition)
	{
		const float fSafeFrac = CLAMP(fFrac,1.0f, 0.0f); 
		const unsigned int uiMaxIndex = uiPointCount - 1; 
		const float fPointPos = uiMaxIndex * fSafeFrac; 
		float fIdx = 0.0f; 
		const float a = modf(fPointPos, &fIdx); 
		const unsigned int iIdx = (int)fIdx; 

		if(iIdx < uiMaxIndex)
		{
			const float aSq = a * a; 
			const float b = (1.0f - a); 
			const float bSq = b * b; 

			Vector3f pA = m_Points[iIdx]; 
			Vector3f pB = m_Direction[iIdx] + m_Points[iIdx]; 
			Vector3f pC = m_Points[iIdx + 1] - m_Direction[iIdx + 1]; 
			Vector3f pD = m_Points[iIdx + 1]; 

			*pPosition = 
				(pD * (a * aSq)) +
				(pC * (3 * aSq * b)) + 
				(pB * (3 * a * bSq)) + 
				(pA * (b * bSq)); 
		
			return S_OK; 
		}
	}
	return E_FAIL; 
}